09. Solution: Mind The Line
Ok, let's walk through the solution. We'll go through each of the
TODO
tags.
MPC.cpp
N & dt
/**
* TODO: Set N and dt
*/
size_t N = 25;
double dt = 0.05;
Here we had to assign values to
N
and
dt
. It's likely you set these variables to slightly different values. That's fine as long as the cross track error decreased to 0. It's a good idea to play with different values here.
For example, if we were to set
N
to 100, the simulation would run much slower. This is because the solver would have to optimize 4 times as many control inputs. Ipopt, the solver, permutes the control input values until it finds the lowest cost. If you were to open up Ipopt and plot the x and y values as the solver mutates them, the plot would look like a worm moving around trying to fit the shape of the reference trajectory.
Cost function
void operator()(ADvector& fg, const ADvector& vars) {
// The cost is stored is the first element of `fg`.
// Any additions to the cost should be added to `fg[0]`.
fg[0] = 0;
// Cost function
/**
* TODO: Define the cost related the reference state and
* anything you think may be beneficial.
*/
// The part of the cost based on the reference state.
for (int t = 0; t < N; t++) {
fg[0] += CppAD::pow(vars[cte_start + t], 2);
fg[0] += CppAD::pow(vars[epsi_start + t], 2);
fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);
}
// Minimize the use of actuators.
for (int t = 0; t < N - 1; t++) {
fg[0] += CppAD::pow(vars[delta_start + t], 2);
fg[0] += CppAD::pow(vars[a_start + t], 2);
}
// Minimize the value gap between sequential actuations.
for (int t = 0; t < N - 2; t++) {
fg[0] += CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
}
There's a lot to unwind here.
Let's start with the function arguments:
fg
and
vars
.
The vector
fg
is where the cost function and vehicle model/constraints is defined. We'll go the
fg
vector in more detail shortly.
The other function argument is the vector
vars
. This vector contains all variables used by the cost function and model:
This is all one long vector, so if
N
is 25 then the indices are assigned as follows:
vars[0], …, vars[24] -> x_1, …, x_{25}
vars[25], …, vars[49] -> y_1, …, y_{25}
vars[50], …, vars[74] -> \psi_1, …, \psi_{25}
vars[75], …, vars[99] -> v_1, …, v_{25}
vars[100], …, vars[124] -> cte_1, …, cte_{25}
vars[125], …, vars[149] -> e\psi_1, …, e\psi_{25}
vars[150], …, vars[173] -> \delta_1, …, \delta_{24}
vars[174], …, vars[197] -> a_1, …, a_{24}
Now let's focus on the actual cost function. Since 0 is the index at which
Ipopt
expects
fg
to store the cost value, we sum all the components of the cost and store them at index 0.
In each iteration through the loop, we sum three components to reach the aggregate cost: our cross-track error, our heading error, and our velocity error.
// The part of the cost based on the reference state.
for (int t = 0; t < N; ++t) {
fg[0] += CppAD::pow(vars[cte_start + t] , 2);
fg[0] += CppAD::pow(vars[epsi_start + t], 2);
fg[0] += CppAD::pow(vars[v_start + t], 2);
}
We've already taken care of the main objective - to minimize our cross track, heading, and velocity errors. A further enhancement is to constrain erratic control inputs.
For example, if we're making a turn, we'd like the turn to be smooth, not sharp. Additionally, the vehicle velocity should not change too radically.
// Minimize change-rate.
for (int t = 0; t < N - 1; ++t) {
fg[0] += CppAD::pow(vars[delta_start + t], 2);
fg[0] += CppAD::pow(vars[a_start + t], 2);
}
The goal of this final loop is to make control decisions more consistent, or smoother. The next control input should be similar to the current one.
// Minimize the value gap between sequential actuations.
for (int t = 0; t < N - 2; ++t) {
fg[0] += CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
}
Initialization & constraints
We initialize the model to the initial state. Recall
fg[0]
is reserved for the cost value, so the other indices are bumped up by 1.
fg[1 + x_start] = vars[x_start];
fg[1 + y_start] = vars[y_start];
fg[1 + psi_start] = vars[psi_start];
fg[1 + v_start] = vars[v_start];
fg[1 + cte_start] = vars[cte_start];
fg[1 + epsi_start] = vars[epsi_start];
All the other constraints based on the vehicle model:
x_{t+1} = x_t + v_t cos(\psi_t) * dt
y_{t+1} = y_t + v_t sin(\psi_t) * dt
\psi_{t+1} = \psi_t + \frac {v_t} { L_f} \delta_t * dt
v_{t+1} = v_t + a_t * dt
cte_{t+1} = f(x_t) - y_t + (v_t sin(e\psi_t) dt)
e\psi_{t+1} = \psi_t - \psi{des}_t + (\frac{v_t} { L_f} \delta_t dt)
Let's look how to model \psi . Based on the above equations, we need to constrain the value of \psi at time t+1:
\psi_{t+1} = \psi_t + \frac {v_t} { L_f} \delta_t * dt
We do that by setting a value within
fg
to the difference of
ps1
and the above formula.
Previously, we have set the corresponding
constraints_lowerbound
and the
constraints_upperbound
values to 0. That means the solver will force this value of
fg
to always be 0.
for (int t = 1; t < N ; ++t) {
// psi, v, delta at time t
AD<double> psi0 = vars[psi_start + t - 1];
AD<double> v0 = vars[v_start + t - 1];
AD<double> delta0 = vars[delta_start + t - 1];
// psi at time t+1
AD<double> psi1 = vars[psi_start + t];
// how psi changes
fg[1 + psi_start + t] = psi1 - (psi0 + v0 * delta0 / Lf * dt);
}
The oddest line above is probably
fg[1 + psi_start + t]
.
fg[0]
stores the cost value, so there's always an offset of 1. So
fg[1 + psi_start]
is where we store the initial value of
\psi
. Finally,
fg[1 + psi_start + t]
is reserved for the
t
th of
N
values of
\psi
that the solver computes.
Coding up the other parts of the model is similar.
for (int t = 1; t < N; ++t) {
// The state at time t+1 .
AD<double> x1 = vars[x_start + t];
AD<double> y1 = vars[y_start + t];
AD<double> psi1 = vars[psi_start + t];
AD<double> v1 = vars[v_start + t];
AD<double> cte1 = vars[cte_start + t];
AD<double> epsi1 = vars[epsi_start + t];
// The state at time t.
AD<double> x0 = vars[x_start + t - 1];
AD<double> y0 = vars[y_start + t - 1];
AD<double> psi0 = vars[psi_start + t - 1];
AD<double> v0 = vars[v_start + t - 1];
AD<double> cte0 = vars[cte_start + t - 1];
AD<double> epsi0 = vars[epsi_start + t - 1];
// Only consider the actuation at time t.
AD<double> delta0 = vars[delta_start + t - 1];
AD<double> a0 = vars[a_start + t - 1];
AD<double> f0 = coeffs[0] + coeffs[1] * x0;
AD<double> psides0 = CppAD::atan(coeffs[1]);
// Here's `x` to get you started.
// The idea here is to constraint this value to be 0.
//
// Recall the equations for the model:
// x_[t] = x[t-1] + v[t-1] * cos(psi[t-1]) * dt
// y_[t] = y[t-1] + v[t-1] * sin(psi[t-1]) * dt
// psi_[t] = psi[t-1] + v[t-1] / Lf * delta[t-1] * dt
// v_[t] = v[t-1] + a[t-1] * dt
// cte[t] = f(x[t-1]) - y[t-1] + v[t-1] * sin(epsi[t-1]) * dt
// epsi[t] = psi[t] - psides[t-1] + v[t-1] * delta[t-1] / Lf * dt
fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
fg[1 + psi_start + t] = psi1 - (psi0 + v0 * delta0 / Lf * dt);
fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
fg[1 + cte_start + t] =
cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
fg[1 + epsi_start + t] =
epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);
}
main.cpp
Fitting a polynomial to the waypoints
/**
* TODO: fit a polynomial to the above x and y coordinates
*/
auto coeffs = polyfit(ptsx, ptsy, 1);
The x and y coordinates are contained in the
ptsx
and
ptsy
vectors. Since these are 2-element vectors a 1-degree polynomial (straight line) is sufficient.
Calculating the cross track and orientation error
double x = -1;
double y = 10;
double psi = 0;
double v = 10;
/**
* TODO: calculate the cross track error
*/
double cte = polyeval(coeffs, x) - y;
/**
* TODO: calculate the orientation error
*/
double epsi = psi - atan(coeffs[1]);
The cross track error is calculated by evaluating at polynomial at
x
(-1) and subtracting
y
.
Recall orientation error is calculated as follows e\psi = \psi - \psi{des} , where \psi{des} is can be calculated as arctan(f'(x)) .
f(x) = a_0 + a_1*x
f'(x) = a_1
hence the solution
double epsi = psi - atan(coeffs[1]);